#include "CudaForceComputer.cuh"
#include "MagnetConstVariable.h"

__device__ void devFmagnetForce(float *pos, float *mm, float *mindist_pos, float *mindist_mm, float *out, float k){

	float nor[2];
	nor[0] = pos[0] - mindist_pos[0];
	nor[1] = pos[1] - mindist_pos[1];

	float dist = nor[0] * nor[0] + nor[1] * nor[1];
	

	float sqrtdist = sqrt(dist);
		nor[0] /= sqrtdist;
		nor[1] /= sqrtdist;

		dist += SCALE_CALFORCE_MINDISTANCE;

		float mkdotnor = mm[0] * nor[0] + mm[1] * nor[1];
		float midotnor = mindist_mm[0] * nor[0] + mindist_mm[1] * nor[1];
		float mmdot = mm[0] * mindist_mm[0] + mm[1] * mindist_mm[1];


		float temp[2];
		temp[0] = -5 * nor[0] * mkdotnor * midotnor;
		temp[1] = -5 * nor[1] * mkdotnor * midotnor;

		temp[0] += nor[0] * mmdot;
		temp[1] += nor[1] * mmdot;

		temp[0] += (mm[0] * midotnor + mindist_mm[0] * mkdotnor);
		temp[1] += (mm[1] * midotnor + mindist_mm[1] * mkdotnor);

		out[0] += 3 * k * temp[0] * powf(dist, -SCALE_CALFORCE_INDEX) * SCALE_FAKEMAGNET_FORCE;
		out[1] += 3 * k * temp[1] * pow(dist, -SCALE_CALFORCE_INDEX) * SCALE_FAKEMAGNET_FORCE;





}//devForce

__global__ void gloFmagnetForce(float *pos, float *mm, float *mindist_pos, float *mindist_mm, float *force, int numParticle, float k){

	int tid = KERNEL_INDEX;

	if(tid<numParticle){

		int x,y;
		x = tid*2;
		y = x+1;

		/*float nor[2];
		nor[0] = pos[x] - mindist_pos[x];
		nor[1] = pos[y] - mindist_pos[y];

		float dist = nor[0] * nor[0] + nor[1] * nor[1];
		if(dist != 0){

		dist = sqrt(dist);

		nor[0] /= dist;
		nor[1] /= dist;

		float mkdotnor = mm[x] * nor[0] + mm[y] * nor[1];
		float midotnor = mindist_mm[x] * nor[0] + mindist_mm[y] * nor[1];
		float mmdot = mm[x] + mindist_mm[x] + mm[y] + mindist_mm[y];


		float temp[2];
		temp[0] = -15 * nor[0] * mkdotnor * midotnor;
		temp[1] = -15 * nor[1] * mkdotnor * midotnor;

		temp[0] += 3 * nor[0] * mmdot;
		temp[1] *= 3 *nor[1] * mmdot;

		temp[0] += 3 * (mm[x] * midotnor + mindist_mm[x] * mkdotnor);
		temp[1] += 3 * (mm[y] * midotnor + mindist_mm[y] * mkdotnor);

		force[x] += temp[0] * powf(dist, -4);
		force[y] += temp[1] * pow(dist, -4);*/


		devFmagnetForce(pos + x, mm + x, mindist_pos + x, mindist_mm + x, force + x, k);

}//if < numParticle

}//devStart

__device__ void devMindist(float *pos, float *Fmagnet_pos, float *Fmagnet_mm, int numFmagnet, float *out){

	float nor[2];

	float dist2;
	int x, y;


	for(int i = 0; i<numFmagnet; i++){
		
		x = 2 * i;
		y = x+1;

		nor[0] = pos[0] - Fmagnet_pos[x];
		nor[1] = pos[1] - Fmagnet_pos[y];

		dist2 = nor[0] * nor[0] + nor[1] * nor[1];

		if(out[4] > dist2){

			
			out[0] = Fmagnet_pos[x];
			out[1] = Fmagnet_pos[y];
			out[2] = Fmagnet_mm[x];
			out[3] = Fmagnet_mm[y];
			out[4] = dist2;



		}//if new min


	}//for numFmagnet

	
}//devMindist

__global__ void gloMindist(float *pos, int numParticle, float gapx, float gapy, int numGridx, int numGridy, int *ptrFmagnet, float *Fmagnet_pos, float *Fmagnet_mm, float *mindist_pos, float *mindist_mm){

	int tid = KERNEL_INDEX;

	if(tid < numParticle){


		float  out[5];
		
		out[0] = 1.f;
		out[1] = 9.f;
		out[2] = 0.f;
		out[3] = 0.f;
		out[4] = 99.f;

		int x, y;
		x = tid *2;
		y = x+1;

		int pGrid[2];

		pGrid[0] = pos[x]/gapx;
		pGrid[1] = pos[y]/gapy;


		int startx, starty, endx, endy;

		if(pGrid[0] >1)
			startx = pGrid[0]-1;
		else
			startx = pGrid[0];

		if(pGrid[1] > 1)
			starty = pGrid[1] -1;
		else
			starty = pGrid[1];

		if(pGrid[0] < numGridx-1)
			endx = pGrid[0] +1;
		else
			endx = pGrid[0];

		if(pGrid[1] < numGridy - 1)
			endy = pGrid[1] +1;
		else
			endy = pGrid[1];

		int start, end;
		int num;
		for(int py = starty; py <= endy; py++){
			start = ptrFmagnet[py * numGridx + startx];
			end = ptrFmagnet[py * numGridx + endx];

			num = end - start + 1;

			devMindist(pos + x, Fmagnet_pos + start*2, Fmagnet_mm + start*2, num, out);

		}//for py

		mindist_pos[x] = out[0];
		mindist_pos[y] = out[1];
		mindist_mm[x] = out[2];
		mindist_mm[y] = out[3];


	}//if < numParticle



}//gloMindist

__device__ void devSetMfield(float* magnet_pos, float* magnet_mm, float* pos, float* out, int numMagnet, float k) 
{

	int i;
	out[0] = 0; out[1] = 0;
	int last = numMagnet * 2;
	for (i=0; i<last; i+=2) {
		float nor[2];
		nor[0] = pos[0] - *(magnet_pos++);
		nor[1] = pos[1] - *(magnet_pos++);

		float dist = (nor[0] * nor[0] + nor[1] * nor[1]);
		dist = sqrtf(dist);
		if (dist < 0.02f) continue;
		nor[0] /= dist;
		nor[1] /= dist;

		dist = powf(dist, -3);
		float factor = 3 * (nor[0] * magnet_mm[0] + nor[1] * magnet_mm[1]);
		out[0] += (nor[0] * factor - *(magnet_mm++)) * dist;
		out[1] += (nor[1] * factor - *(magnet_mm++)) * dist;
	}
	out[0] *= k;
	out[1] *= k;
}

__global__ void gloSetMfield(float *Magnet_pos, float *Magnet_mm, int numMagnet, float *mField, float domain, float gap, int nummField, float k){

	int tid = KERNEL_INDEX;

	if(tid < nummField){

		int numx = (int)(domain/gap);

		int Y = tid/numx;
		int X = tid - Y * numx;

		float pos[2];
		pos[0] = X * gap;
		pos[1] = Y * gap;


		devSetMfield(Magnet_pos, Magnet_mm, pos, mField + tid*2, numMagnet, k);




	}//if < nummField

}//gloSetMfield


__device__ void devSetForce(float* out, float* pos, float* mm, float* magnet_pos, float* magnet_mm, int numMagnet, float k) 
{
	int i;
	out[0] = 0; out[1] = 0;
	int last = numMagnet * 2;
	for (i=0; i<last; i+=2) {
		float nor[2];
		nor[0] = pos[0] - *(magnet_pos++);
		nor[1] = pos[1] - *(magnet_pos++);

		float dist = (nor[0] * nor[0] + nor[1] * nor[1]);
		dist = sqrtf(dist);
		if (dist < 0.02f) continue;
		nor[0] /= dist;
		nor[1] /= dist;
		dist = powf(dist, -4) * 3;
		float mmidotnor = magnet_mm[0] * nor[0] + magnet_mm[1] * nor[1];
		float mmkdotnor = mm[0] * nor[0] + mm[1] * nor[1];
		float mmkdotmmi = magnet_mm[0] * mm[0] + magnet_mm[1] * mm[1];
		for (int j=0; j<2; j++) {
			float temp = -5 * nor[j] * mmkdotnor * mmidotnor;
			temp += nor[j] * mmkdotmmi;
			temp += mm[j] * mmidotnor;
			temp += magnet_mm[j] * mmkdotnor;
			out[j] +=  temp * dist;
		}
		magnet_mm+=2;
	}
	out[0] *= k;
	out[1] *= k;
}


__global__ void gloSetFfield(float *Ffield,  float *Magnet_pos, float *Magnet_mm, float *mField, int numMagnet, int nummField, float domain, float gap, float inducefactor, float k){


	int tid = KERNEL_INDEX;

	if(tid < nummField){

		int numx = (int)(domain/gap);

		int Y = tid/numx;
		int X = tid - Y * numx;

		float pos[2];
		float mm[2];

		pos[0] = X * gap;
		pos[1] = Y * gap;

		float* mf = mField + tid;
		mm[0] = mf[0] *inducefactor;
		mm[1] = mf[1] * inducefactor;

		

		devSetForce(Ffield + tid *2, pos, mm, Magnet_pos, Magnet_mm, numMagnet, k);




	}//if < nummField




}//gloSetFfield


__global__ void gloComputePTCvalue(float *pos, float *mm_out, float *force_out, float *mField, float *FField, int numParticle, float domain, float gap){

	int tid = KERNEL_INDEX;

	if(tid < numParticle){

		int numx = (int)(domain/gap);


		int X = pos[tid * 2]/gap;
		int Y = pos[tid * 2 +1]/gap;

		if(X == numx-1){
			pos[tid * 2] -= gap/10.f;
			X -=1;
		}
		if(Y == numx-1){
			pos[tid * 2+1] -= gap/10.f;
			Y -=1;
		}

		float ax, bx;

		ax = pos[tid * 2] - ((float)X * gap);
		bx = gap - ax;
		ax /= gap;
		bx /= gap;

		int idx1, idx2;
		idx1 = Y * numx + X;
		idx1 *=2;
		idx2 = idx1 + 2;

		float mm_x_temp1 = ax * mField[idx2] + bx * mField[idx1];
		float mm_y_temp1 = ax * mField[idx2+1] + bx * mField[idx1+1];
		float Force_x_temp1 = ax * FField[idx2] + bx * FField[idx1];
		float Force_y_temp1 = ax * FField[idx2+1] + bx * FField[idx1+1];

		idx1 += numx*2;
		idx2 += numx*2;

		float mm_x_temp2 = ax * mField[idx2] + bx * mField[idx1];
		float mm_y_temp2 = ax * mField[idx2+1] + bx * mField[idx1+1];
		float Force_x_temp2 = ax * FField[idx2] + bx * FField[idx1];
		float Force_y_temp2 = ax * FField[idx2+1] + bx * FField[idx1+1];
		
		ax = pos[tid*2+1] - ((float)Y * gap);
		bx = gap- ax;
		ax /= gap;
		bx /= gap;

		float mm_x_out = ax * mm_x_temp2 + bx * mm_x_temp1;
		float mm_y_out = ax * mm_y_temp2 + bx * mm_y_temp1;
		float Force_x_out = ax * Force_x_temp2 + bx * Force_x_temp1;
		float Force_y_out = ax * Force_y_temp2 + bx * Force_y_temp1;


		
		mm_out[tid * 2] = mm_x_out;
		mm_out[tid * 2 +1] = mm_y_out;
		force_out[tid * 2] = Force_x_out;
		force_out[tid * 2 +1] = Force_y_out;

	}//if tid < num

}//gloComputeMm



	void CudaForceComputer::ComputeFmagnetForce(){

		KERNEL_CALL_DECLEAR(m_numParticle);

		gloMindist<<<KERNEL_CALL>>> (dev_m_pos, m_numParticle, m_gapx, m_gapy, m_numGridx, m_numGridy, dev_m_ptrFmagnet, dev_m_Fmagnet_pos, dev_m_Fmagnet_mm, dev_m_mindist_pos, dev_m_mindist_mm);

		gloFmagnetForce<<<KERNEL_CALL>>> (dev_m_pos, dev_m_mm, dev_m_mindist_pos, dev_m_mindist_mm, dev_m_force, m_numParticle, m_k);

		
	}//start


	void CudaForceComputer::SetField(float *dev_Magnet_pos, float *dev_Magnet_mm){

		KERNEL_CALL_DECLEAR(m_numParticle);

		float domain = m_numGridx * m_gapx;
		float gap = m_gapx/m_scale;


		gloSetMfield<<<KERNEL_CALL>>>(dev_Magnet_pos, dev_Magnet_mm, m_numMagnet, dev_m_mField, domain, gap, m_numMfield, m_k);
		
		
		gloSetFfield<<<KERNEL_CALL>>>(dev_m_FField, dev_Magnet_pos, dev_Magnet_mm, dev_m_mField, m_numMagnet, m_numMfield, domain, gap, m_inducefactor, m_k);


	}//SetMfield()





	void CudaForceComputer::ComputePTCvalue(){

		float domain = m_numGridx * m_gapx;
		float gap = m_gapx/m_scale;
	
		KERNEL_CALL_DECLEAR(m_numParticle);

		gloComputePTCvalue<<<KERNEL_CALL>>>(dev_m_pos, dev_m_mm, dev_m_force, dev_m_mField, dev_m_FField, m_numParticle, domain, gap);



	}//ComputePTCvalue